#include "BambooKicker.h"
#include "math.h"

const int windVal = 40;
const double firingLength = .5;
const double latchTime = .5;

BambooKicker::BambooKicker(UINT32 winch, UINT32 encoder1, UINT32 encoder2, UINT32 solenoid, GenericHID *joystick, DigitalInput *kickerswitch, int kickerbutton) {
	m_winch = new Victor(winch);
	m_solenoid = new Solenoid(solenoid);
	m_encoder = new Encoder(encoder1, encoder2, false);
	m_encoder->Reset();
	m_encoder->Start();
	m_joystick = joystick;
	m_kickerswitch = kickerswitch;
	m_kickerbutton = kickerbutton;
	m_waiting = false;
}

BambooKicker::~BambooKicker(void) {
	
}

int BambooKicker::Kick(bool kickButton) {
	static int state = fired;
	static int counter = 0;
	double curTime = GetTime();
	/*
	if (counter % 100 == 0) {
		printf("State : %d", state);
		printf("Switch : %d", m_kickerswitch->Get());
		printf("Waiting : %d\n", m_waiting);
		printf("Button : %d", m_joystick->GetRawButton(m_kickerbutton));
	}*/

	switch(state) {
		case fired:
			m_solenoid->Set(true);
			if(m_kickerswitch->Get() == 1)
				m_winch->Set(1.0);
			else {
				m_solenoid->Set(false);
				m_Time = GetTime();
				state = latching;
			}
			break;
		case latching:
			if(m_kickerswitch->Get() == 1) {
				state = fired;
				break;
			}
			if(curTime - m_Time > latchTime) {
				m_winch->Set(0);
				state = wound;
			}
			break;	
		case wound:
			if(m_kickerswitch->Get() == 1) {
				state = fired;
				break;
			}
			if(m_encoder->Get() > 30 )
				m_winch->Set(-.2);
			else {
				m_winch->Set(0);
				state = primed;
			}
			break;
		case primed:
			if (m_waiting) {
				if (curTime - m_Time >= firingLength) {
					state = fired;
					m_encoder->Reset();
					m_waiting = false;
					break;
				}
			} else if (kickButton) {
				m_solenoid->Set(true);
				m_Time = GetTime();
				m_waiting = true;
			}
		break;	
	}
	counter++;
	return state;
}

int BambooKicker::Teleop() {
	int kickingState;
	kickingState = Kick(m_joystick->GetRawButton(m_kickerbutton));
	return kickingState;
}
